﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Runtime.InteropServices;
using System.Text;

namespace NIP.ControllerInterface
{
    #region Function Pointers (Delegates)

    /// <summary>
    /// Movement delegate for the robot's movement callbacks.
    /// </summary>
    /// <param name="axis">The axis being moved.</param>
    [UnmanagedFunctionPointer(CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
    internal delegate void MovementCallback(byte axis);

    /// <summary>
    /// WatchControl delegate for the robot's watch-control callback.
    /// </summary>
    /// <param name="axisStatus">Axis status from the current control status.</param>
    [UnmanagedFunctionPointer(CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
    internal delegate void WatchControlCallback(uint axisStatus);

    /// <summary>
    /// HomingNotifier delegate for the robot's homing-notifier callback.
    /// </summary>
    /// <param name="homingStatus">Homing status code.</param>
    [UnmanagedFunctionPointer(CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
    internal delegate void HomingNotifierCallback(byte homingStatus);

    /// <summary>
    /// WatchJoint delegate for the robot's watch-joint callback.
    /// </summary>
    /// <param name="robotData">The robots joint data.</param>
    [UnmanagedFunctionPointer(CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
    internal delegate void WatchJointCallback(int[] robotData);

    /// <summary>
    /// IOMonitor delegate for the robot's digital input/output monitoring callbacks.
    /// </summary>
    /// <param name="status">IO status code.</param>
    [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
    internal delegate void IOMonitorCallback(int status);

    /// <summary>
    /// InitializationCompleted delegate for the robot's initialization-completed callback.
    /// </summary>
    /// <param name="configuration">Robot configuration.</param>
    [UnmanagedFunctionPointer(CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
    internal delegate void InitializationCompleteCallback(Structs.ConfigData configuration);

    /// <summary>
    /// Error delegate for the robot's error callback.
    /// </summary>
    /// <param name="error">Error information.</param>
    [UnmanagedFunctionPointer(CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
    internal delegate void ErrorCallback(USBCError error);

    #endregion

    public class Wrapper
    {
        private const string PATH = "USBC.dll";

        #region Setup
        //Initialization
        [DllImport(PATH, EntryPoint = "?Initialization@@YAHFFP6AXPAX@Z1@Z",
                                                    CallingConvention = CallingConvention.Cdecl)]
        public static extern bool Initialization(short sMode, short sSystemType,
                                                    Delegate fnInitEnd, Delegate fnErrMessage);

        //Control
        [DllImport(PATH, EntryPoint = "?Control@@YAHEH@Z", CallingConvention = CallingConvention.Cdecl)]
        public static extern bool Control(byte ucGroupAxis, bool bIsOn);


        //Home
        [DllImport(PATH, EntryPoint = "?Home@@YAHEP6AXPAX@Z@Z", CallingConvention = CallingConvention.Cdecl)]
        public static extern bool Home(byte ucGroupAxis, Delegate fnHomingNotif);

        #endregion
        #region Input/Output

        /// <summary>
        /// Begins monitoring the digital input.
        /// </summary>
        /// <param name="monitorCallback">Callback used to watch the digital input.</param>
        /// <returns>true if operation was succesful; otherwise false.</returns>
        [DllImport(PATH, EntryPoint = "?WatchDigitalInp@@YAHP6AXPAX@Z@Z",
            CallingConvention = CallingConvention.Cdecl)]
        [return: MarshalAs(UnmanagedType.Bool)]
        internal static extern bool BeginMonitorDigitalInput(IOMonitorCallback monitorCallback);

        /// <summary>
        /// Stops monitoring the digital input.
        /// </summary>
        /// <returns>true if operation was succesful; otherwise false.</returns>
        [DllImport(PATH, EntryPoint = "?CloseWatchDigitalInp@@YAHXZ",
            CallingConvention = CallingConvention.Cdecl)]
        [return: MarshalAs(UnmanagedType.Bool)]
        internal static extern bool EndWatchDigitalInput();

        /// <summary>
        /// Begins monitoring the digital output.
        /// </summary>
        /// <param name="monitorCallback">Callback used to watch the digital output.</param>
        /// <returns>true if operation was succesful; otherwise false.</returns>
        [DllImport(PATH, EntryPoint = "?WatchDigitalOut@@YAHP6AXPAX@Z@Z",
            CallingConvention = CallingConvention.Cdecl)]
        [return: MarshalAs(UnmanagedType.Bool)]
        internal static extern bool BeginMonitorDigitalOutput(IOMonitorCallback monitorCallback);

        /// <summary>
        /// Stops monitoring the digital output.
        /// </summary>
        /// <returns>true if operation was succesful; otherwise false.</returns>
        [DllImport(PATH, EntryPoint = "?CloseWatchDigitalOut@@YAHXZ",
            CallingConvention = CallingConvention.Cdecl)]
        [return: MarshalAs(UnmanagedType.Bool)]
        internal static extern bool EndMonitorDigitalOutput();

        /// <summary>
        /// Gets the digital input.
        /// </summary>
        /// <param name="statusBitmap">Digital input status bitmap.</param>
        /// <returns>true if operation was succesful; otherwise false.</returns>
        [DllImport(PATH, EntryPoint = "?GetDigitalInputs@@YAHPAK@Z",
            CallingConvention = CallingConvention.Cdecl)]
        [return: MarshalAs(UnmanagedType.Bool)]
        internal static extern bool GetDigitalInput([Out] uint statusBitmap);

        /// <summary>
        /// Gets the digital output.
        /// </summary>
        /// <param name="statusBitmap">Digital output status bitmap.</param>
        /// <returns>true if operation was succesful; otherwise false.</returns>
        [DllImport(PATH, EntryPoint = "?GetDigitalOutputs@@YAHPAK@Z",
            CallingConvention = CallingConvention.Cdecl)]
        [return: MarshalAs(UnmanagedType.Bool)]
        internal static extern bool GetDigitalOutput([Out] uint statusBitmap);

        /// <summary>
        /// Sets the digital output.
        /// </summary>
        /// <param name="ioNumber">Output value.</param>
        /// <param name="isOn">Toggles on/off.</param>
        /// <param name="isImmediate">Sets of the operation is immediate or not.</param>
        /// <returns>true if operation was succesful; otherwise false.</returns>
        [DllImport(PATH, EntryPoint = "?SetDigitalOutput@@YAHFHH@Z",
            CallingConvention = CallingConvention.Cdecl)]
        [return: MarshalAs(UnmanagedType.Bool)]
        internal static extern bool SetDigitalOutput(short ioNumber,
            [MarshalAs(UnmanagedType.Bool)] bool isOn,
            [MarshalAs(UnmanagedType.Bool)] bool isImmediate);

        #endregion


        #region Movement

        #region Stop function

        //Stop
        [DllImport(PATH, EntryPoint = "?Stop@@YAHE@Z", CallingConvention = CallingConvention.Cdecl)]
        public static extern bool Stop(byte ucGroup);

        #endregion

        #region Gripper Functions

        // OpenGripper
        [DllImport(PATH, EntryPoint = "?OpenGripper@@YAHXZ", CallingConvention = CallingConvention.Cdecl)]
        public static extern bool OpenGripper();

        //CloseGripper
        [DllImport(PATH, EntryPoint = "?CloseGripper@@YAHXZ", CallingConvention = CallingConvention.Cdecl)]
        public static extern bool CloseGripper();

        //GetJaw
        [DllImport(PATH, EntryPoint = "?GetJaw@@YAHPAF0@Z", CallingConvention = CallingConvention.Cdecl)]
        public static extern bool GetJaw(ref short percent, ref short mm);

        #endregion

        #region Robot movement

        //DefineVector
        [DllImport(PATH, EntryPoint = "?DefineVector@@YAHEPADF@Z", CallingConvention = CallingConvention.Cdecl)]
        public static extern bool DefineVector(byte ucGroup, string szVectorName, short sDimension);

        [DllImport(PATH, EntryPoint = "?CloseTorque@@YAHXZ", CallingConvention = CallingConvention.Cdecl)]
        public static extern bool CloseTorque();
        
        //Here
        [DllImport(PATH, EntryPoint = "?Here@@YAHPADFJ@Z", CallingConvention = CallingConvention.Cdecl)]
        public static extern bool Here(string szVectorName, short sPointNumber, long lPointType);

        //Teach
        [DllImport(PATH, EntryPoint = "?Teach@@YAHPADFPAJFJ@Z", CallingConvention = CallingConvention.Cdecl)]
        public static extern bool Teach(string szVectorName, short sPointNumber, int[] plCoolArray, short sCoorArrayDim, long lPointType);

        //MoveLinear
        [DllImport(PATH, EntryPoint = "?MoveLinear@@YAHPADF0F@Z", CallingConvention = CallingConvention.Cdecl)]
        public static extern bool MoveLinear(string szVectorName, short sPointNumber, string szVectorNameB, short sPointNumberB);

        [DllImport(PATH, EntryPoint = "?MoveTorque@@YAHEJ@Z", CallingConvention = CallingConvention.Cdecl)]
        public static extern bool MoveTorque(byte axis, long torque);

        [DllImport(PATH, EntryPoint = "?MoveTorque@@YAHPAJF@Z", CallingConvention = CallingConvention.Cdecl)]
        //public static extern bool MoveTorque(long[] plTorqueArray, short sTorqueArrayDim);
        public static extern bool MoveTorque(int[] plTorqueArray, short sTorqueArrayDim);


        /// <summary>
        /// SetJoints
        /// </summary>
        /// <param name="vectorName">Vector name.</param>
        /// <param name="pointNumber">Vector point number.</param>
        /// <param name="coordinates">Coordinates array.</param>
        /// <param name="dimension">Length of the coordinates array.</param>
        /// <param name="pointType">Vector point type.</param>
        /// <returns><c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.</returns>
        [DllImport(PATH, EntryPoint = "?SetJoints@@YAHPADFPAJFJ@Z",
            CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Unicode)]
        public static extern bool SetJoints(string vectorName, short pointNumber,
            int[] coordinates, short dimension, int pointType);

        [DllImport(PATH, EntryPoint = "?SavePoints@@YAHPAD@Z",
                CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Unicode)]
        public static extern bool SavePoints(string fileName);

        /// <summary>
        /// Moves the robot without care for path
        /// </summary>
        /// <remarks>
        /// If <paramref name="vectorNameB"/> and <paramref name="pointNumberB"/> are set to null and 0 respectively, 
        /// the robot only moves to the first position.
        /// </remarks>
        /// <param name="vectorName">First vector name.</param>
        /// <param name="pointNumber">First vector point number.</param>
        /// <param name="vectorNameB">Second vector name.</param>
        /// <param name="pointNumberB">Second vector point number.</param>
        /// <returns><c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.</returns>
        [DllImport(PATH, EntryPoint = "?MoveJoint@@YAHPADF0F@Z",
            CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Unicode)]
        public static extern bool MoveJoint(string vectorName, short pointNumber,
            string vectorNameB, short pointNumberB);

        #endregion

        #region Status

        //IsOnlineOk
        [DllImport(PATH, EntryPoint = "?IsOnLineOk@@YAHXZ", CallingConvention = CallingConvention.Cdecl)]
        public static extern bool IsOnlineOk();

        //GetCurrentPosition
        [DllImport(PATH, EntryPoint = "?GetCurrentPosition@@YAHPAY07J00@Z", CallingConvention = CallingConvention.Cdecl)]
        public static extern bool GetCurrentPosition(int[] pEnc, int[] pJoint, int[] pXYZ);

        [DllImport(PATH, EntryPoint = "?GetPointInfo@@YAHPADFPAY07J11PAJ@Z", CallingConvention = CallingConvention.Cdecl)]
        public static extern bool GetPointInfo(string szVectorName, short sPointNumber,
            int[] pEncData, int[] pXYZData, int[] lPointType);
        #endregion

        #region Manual movement

        //EnterManual
        [DllImport(PATH, EntryPoint = "?EnterManual@@YAHF@Z", CallingConvention = CallingConvention.Cdecl)]
        public static extern bool EnterManual(short sManualType);

        //CloseManual
        [DllImport(PATH, EntryPoint = "?CloseManual@@YAHXZ", CallingConvention = CallingConvention.Cdecl)]
        public static extern bool CloseManual();

        //MoveManual
        [DllImport(PATH, EntryPoint = "?MoveManual@@YAHEJ@Z", CallingConvention = CallingConvention.Cdecl)]
        public static extern bool MoveManual(byte ucAxis, long lVelocity);

        #endregion

        #region Time and Speed

        //Time
        [DllImport(PATH, EntryPoint = "?Time@@YAHEJ@Z", CallingConvention = CallingConvention.Cdecl)]
        public static extern bool Time(byte ucGroup, long lTime);

        //Speed
        [DllImport(PATH, EntryPoint = "?Speed@@YAHEJ@Z", CallingConvention = CallingConvention.Cdecl)]
        public static extern bool Speed(byte ucGroup, long lSpeed);

        #endregion

        #region Monitoring
        /// <summary>
        /// Sets the callback functions for movement events.
        /// </summary>
        /// <remarks>
        /// The value of this char denotes on what part of the robot the event has occured: 
        ///     '0'-'7' for axis movements 
        ///     'A'     for robot movements 
        ///     'B'     for peripheral movements 
        ///     'G'     for gripper movements 
        /// <para />
        /// If both parameters are set with non null values, a pointer to the previous fnMotionEnd function will be returned.
        /// </remarks>
        /// <param name="motionEnded">Callback for when the motion have ended.</param>
        /// <param name="motionStarted">Callback for when the motion have started.</param>
        /// <returns>Pointer to the previous callback function.</returns>
        [DllImport(PATH, EntryPoint = "?WatchMotion@@YAP6AXPAX@ZP6AX0@Z1@Z",
            CallingConvention = CallingConvention.Cdecl)]
        internal static extern MovementCallback WatchMotion(
            [MarshalAs(UnmanagedType.FunctionPtr)] MovementCallback motionEnded,
            [MarshalAs(UnmanagedType.FunctionPtr)] MovementCallback motionStarted);

        /// <summary>
        /// Watch control status on the robot.
        /// </summary>
        /// <param name="watchControl">Callback used to watch the control changes.</param>
        /// <returns><c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.</returns>
        [DllImport(PATH, EntryPoint = "?WatchControl@@YAHP6AXPAX@Z@Z",
            CallingConvention = CallingConvention.Cdecl)]
        [return: MarshalAs(UnmanagedType.Bool)]
        internal static extern bool WatchControl(WatchControlCallback watchControl);

        /// <summary>
        /// Stops watch control status on the robot.
        /// </summary>
        /// <returns><c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.</returns>
        [DllImport(PATH, EntryPoint = "?CloseWatchControl@@YAHXZ",
            CallingConvention = CallingConvention.Cdecl)]
        [return: MarshalAs(UnmanagedType.Bool)]
        internal static extern bool CloseWatchControl();


        /// <summary>
        /// Watch the homing process.
        /// </summary>
        /// <remarks>
        /// The callback functions parameters tells the status of the homing process.
        /// <para />
        /// The values are: 
        ///     0xff:  Homing started 
        ///     1 - 8: Axis 1 - 8 is being homed 
        ///     0x40:  Homing ended 
        /// <para />
        /// These values does not indicate succes or failure.
        /// </remarks>
        /// <param name="homingNotifier">Callback for when the homing status changes.</param>
        /// <returns><c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.</returns>
        [DllImport(PATH, EntryPoint = "?WatchHoming@@YAHP6AXPAX@Z@Z",
            CallingConvention = CallingConvention.Cdecl)]
        [return: MarshalAs(UnmanagedType.Bool)]
        internal static extern bool WatchHoming(
            [MarshalAs(UnmanagedType.FunctionPtr)] HomingNotifierCallback homingNotifier);


        /// <summary>
        /// Watch the values for each joint on the robot.
        /// </summary>
        /// <remarks>
        /// The interval between each callback is defined in USBC.ini in miliseconds.
        /// </remarks>
        /// <param name="watchJoint">Callback used to watch the joint changes.</param>
        /// <returns><c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.</returns>
        [DllImport(PATH, EntryPoint = "?WatchJoint@@YAHP6AXPAX@Z@Z",
            CallingConvention = CallingConvention.Cdecl)]
        [return: MarshalAs(UnmanagedType.Bool)]
        internal static extern bool WatchJoint(WatchJointCallback watchJoint);

        /// <summary>
        /// Stops watch the joints status on the robot.
        /// </summary>
        /// <returns><c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.</returns>
        [DllImport(PATH, EntryPoint = "?CloseWatchJoint@@YAHXZ",
            CallingConvention = CallingConvention.Cdecl)]
        [return: MarshalAs(UnmanagedType.Bool)]
        internal static extern bool CloseWatchJoint();

        #endregion

        #endregion
    }
}
